import numpy as np
from scipy.linalg import block_diag


def F6_MPC_Matrices_Constraints(x_low, x_high, u_low, u_high, N_P, Phi, Gamma):
    # 计算系统状态维度
    n = x_low.shape[0]
    # 计算系统输入维度
    p = u_low.shape[0]

    # 构建M矩阵
    M = np.vstack([np.zeros((p, n)), np.zeros((p, n)), -np.eye(n), np.eye(n)])
    
    # 构建F矩阵
    F = np.vstack([-np.eye(p), np.eye(p), np.zeros((n, p)), np.zeros((n, p))])
    
    # 构建Beta矩阵
    Beta = np.vstack([-u_low, u_high, -x_low, x_high])
    
    # 构建M_Np矩阵
    M_Np = np.vstack([-np.eye(n), np.eye(n)])
    
    # 构建Beta_N矩阵
    Beta_N = np.vstack([-x_low, x_high])
    
    # 构建M_bar矩阵
    M_bar = np.zeros(((2*n + 2*p)*N_P + 2*n, n))
    M_bar[:(2*n + 2*p), :] = M
    
    # 构建Beta_bar矩阵
    Beta_bar = np.vstack([np.tile(Beta, (N_P, 1)), Beta_N])
    
    # 初始化M_2bar矩阵
    M_2bar = M
    F_2bar = F

    # for循环创建M_2bar和F_2bar矩阵
    for i in range(N_P - 2):
        M_2bar = block_diag(M_2bar, M)
        F_2bar = block_diag(F_2bar, F)

    M_2bar = block_diag(M_2bar, M_Np)
    M_2bar = np.vstack( [np.zeros((2*n+2*p, n*N_P)), M_2bar] )

    # 构建F_2bar矩阵最终形式（加入底部一行的零矩阵）
    F_2bar = block_diag(F_2bar, F)
    F_2bar = np.vstack( [ F_2bar, np.zeros((2*n, p*N_P)) ] )
    
    # 构建b矩阵
    b = -(M_bar + M_2bar @ Phi)
    
    # 构建M矩阵
    M = M_2bar @ Gamma + F_2bar
    
    return M, Beta_bar, b